/*
 * The MIT License (MIT)
 * 
 * Permission is hereby granted, free of charge, to any person obtaining a copy
 * of this software and associated documentation files (the "Software"), to deal
 * in the Software without restriction, including without limitation the rights
 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
 * copies of the Software, and to permit persons to whom the Software is
 * furnished to do so, subject to the following conditions:
 * 
 * The above copyright notice and this permission notice shall be included in
 * all copies or substantial portions of the Software.
 * 
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
 * THE SOFTWARE.
 */

package multiswarm;

import box2dLight.Light;
import box2dLight.PointLight;
import box2dLight.RayHandler;

import com.badlogic.gdx.Gdx;
import com.badlogic.gdx.Application.ApplicationType;
import com.badlogic.gdx.graphics.Color;
import com.badlogic.gdx.graphics.OrthographicCamera;
import com.badlogic.gdx.graphics.Texture;
import com.badlogic.gdx.graphics.g2d.SpriteBatch;
import com.badlogic.gdx.graphics.g2d.TextureRegion;
import com.badlogic.gdx.graphics.glutils.ShaderProgram;
import com.badlogic.gdx.graphics.glutils.ShapeRenderer;
import com.badlogic.gdx.graphics.glutils.ShapeRenderer.ShapeType;
import com.badlogic.gdx.math.Vector2;
import com.badlogic.gdx.math.Vector3;
import com.badlogic.gdx.physics.box2d.Body;
import com.badlogic.gdx.physics.box2d.BodyDef;
import com.badlogic.gdx.physics.box2d.BodyDef.BodyType;
import com.badlogic.gdx.physics.box2d.CircleShape;
import com.badlogic.gdx.physics.box2d.Fixture;
import com.badlogic.gdx.physics.box2d.FixtureDef;
import com.badlogic.gdx.physics.box2d.Joint;
import com.badlogic.gdx.physics.box2d.JointDef;
import com.badlogic.gdx.physics.box2d.PolygonShape;
import com.badlogic.gdx.physics.box2d.World;
import com.badlogic.gdx.physics.box2d.joints.DistanceJointDef;
import com.badlogic.gdx.scenes.scene2d.Actor;
import com.badlogic.gdx.utils.Array;

/*
 * This abstract class is the base class for all multiswarm agents.
 * TODO: document better.
 * 
 * @author ralfm
 */
public abstract class Agent extends Actor {
	protected static final short FILTER_AGENT = 0x0001;
	protected static final short FILTER_SENSOR = 0x0002;
	protected static final short FILTER_AGENT_TWO = 0x0004;
	protected static final short FILTER_AGENT_THREE = 0x0008;

	private final float DEGTORAD = (float) (Math.PI / 180f);

	private float blurAmountX = 0.0f;
	private float blurAmountY = 0.0f;
	
	public String identifierString = "BASE";
	
	public long ticks = 0;
	
	protected Arena arena;
	protected TextureRegion region = null;
	
	public boolean isFollowed = false;
	
	protected boolean updateAgentPosition = true;
	protected boolean isDynamic = true;
	
	private ShapeRenderer shapeRenderer = null;
	
	public Body body = null;
	protected Fixture fixture = null;
	public float initDensity = 0f;
	public float initFriction = 0f;
	public float initRestitution = 0f;
	
	protected Vector3 currentPosition = new Vector3(0,0,0);
	protected float currentAngle;
	
	public Light light = null;
	public boolean enableLight = true;

	public Array<AgentSensor> sensors = new Array<AgentSensor>();
	public Array<AgentJoint> joints = new Array<AgentJoint>();
	
	public Array<Agent> directlyConnectedAgents = new Array<Agent>();
	public Array<Agent> allConnectedAgents = new Array<Agent>();
	
	public boolean enableAdditiveBlending = false; //WARNING: This is extremely slow on Android.
	// ... and probably not implemented right the disableBlending Call takes ages.
	public boolean drawJoints = false;
	
	public float speed = 0.0f;
	public float rotation = 0.0f;
	
    private Array<Agent> agentsConnected = new Array<Agent>();
    private Array<Agent> agentsToAddToList = new Array<Agent>();
    
    /**
     * Default constructor. Needs a reference to the agent's arena.
     * @param arena
     */
    public Agent (Arena arena) {
    	this.arena = arena;
    }

    /**
     * Abstract agentAct. Must be overwritten in concrete Agent implementation.
     * Put all agent logic here.
     * @param delta
     */
    public abstract void agentAct(float delta);

    /**
     * Draw the agent. 
     */
    public void draw (SpriteBatch batch, float parentAlpha) {
    	setX(currentPosition.x-(getWidth()/2f));
    	setY(currentPosition.y-(getWidth()/2f));
    	setRotation((float) (currentAngle*180f/Math.PI)-90);
    	
    	if (arena.camera.frustum.sphereInFrustumWithoutNearFar(currentPosition, getWidth())) {
        	if (region != null && body != null && fixture != null) {
        		if (arena.agentShader != null && blurAmountX != 0.0f && blurAmountY != 0.0f) {
        			batch.flush();
        			
        			float floatingArray[] = {blurAmountX, blurAmountY};
        			arena.agentShader.setUniform2fv("blurSize", floatingArray, 0, 2);
        		} 
            	
            	if (enableAdditiveBlending) batch.enableBlending();
            	
                Color color = getColor();
                batch.setColor(color.r, color.g, color.b, color.a * parentAlpha);
                batch.draw(region, getX(), getY(), getOriginX(), getOriginY(),
                        getWidth(), getHeight(), getScaleX(), getScaleY(), getRotation());
                
                if (enableAdditiveBlending) batch.disableBlending();
                
                if (drawJoints) {
                	if (shapeRenderer == null) {
                		shapeRenderer = new ShapeRenderer();
                	}
                	
                	//draw joints.
                	batch.end();
            		shapeRenderer.setProjectionMatrix(arena.camera.combined);

                	for (AgentJoint joint: joints) {
                		Joint box2DJoint = joint.joint;
                		if (box2DJoint != null) {
                    		Vector2 startVec = box2DJoint.getAnchorA();
                    		Vector2 endVec = box2DJoint.getAnchorB();
                    		
                    		shapeRenderer.begin(ShapeType.Line);
                    		shapeRenderer.line(startVec.x, startVec.y, 0, endVec.x, endVec.y, 0);
                    		shapeRenderer.end();
                		}
                	}
                	
                	batch.begin();
                }
        	}
    	}
    }
    
    /**
     * Agent Act. Don't modify this!
     * 
 	 * There are Setup blocks in act() so that we can instantiate an Agent at all times
     * during simulation (messing with Box2D outside of acts() is bad!)  
     * What we set up:
     *   - Texture
     *   - Body
     *   - Light
     *   - Sensors
     *   - Joints
     * 
     * After that the agent is updated according to the speed and rotation values and
     * agentAct is called.
     */
    public void act (float delta) {
		super.act(delta);
    	ticks++;

		//Setup Texture
		if (region == null) {
			//Texture texture = new Texture(Gdx.files.internal("data/agent_default.png"));
			region = new TextureRegion(arena.agentTexture);
	        setWidth(region.getRegionWidth());
	        setHeight(region.getRegionHeight());
	        setOrigin(getWidth()/2, getHeight()/2);
		} 
		
		//Setup Body
		if (body == null && fixture == null) {
			BodyDef bodyDef = new BodyDef();
			bodyDef.position.set(getX()+(getWidth()/2f), getY()+(getHeight()/2f));
			bodyDef.angle = rotation;
			if (!isDynamic) {
				bodyDef.type = BodyType.KinematicBody;
			} else {
				bodyDef.type = BodyType.DynamicBody;
			}
			
			body = arena.world.createBody(bodyDef);
						
			CircleShape circleShape = new CircleShape();
			circleShape.setRadius(getScaleX()*(getWidth()/2.4f));
			
			//Gdx.app.log("",""+circleShape.getRadius());
			
			FixtureDef circleFixture = new FixtureDef();
			circleFixture.shape = circleShape;
			circleFixture.density = initDensity;
			circleFixture.friction = initFriction;
			circleFixture.restitution = initRestitution;
			
			circleFixture.filter.categoryBits = FILTER_AGENT;
			circleFixture.filter.maskBits = FILTER_AGENT | FILTER_SENSOR;
			//circleFixture.filter.maskBits = FILTER_SENSOR;

			fixture = body.createFixture(circleFixture);
		
			fixture.setUserData(this);
			
		}
		
		//Setup Light
		if (enableLight == false) {
			if (light != null) {
				light.remove();
				light = null;
			}
		} else if (light == null) {
			//Color color = new Color(getColor().r,getColor().g,getColor().b,1.0f);
			//light = new ConeLight(rayhandler, 10, color, 100*getScaleX(), getX(), getY(), 0, 360);
			if (Gdx.app.getType() == ApplicationType.Desktop) {
				light = new PointLight(arena.rayHandler, 20, getColor(), 170*getScaleX(), getX(), getY()); //TODO: Was 500 rays
			} else {
				light = new PointLight(arena.rayHandler, 5, getColor(), 170*getScaleX(), getX(), getY()); //TODO: Was 500 rays				
			}
			
			light.setXray(true);
			//light.attachToBody(body, -getHeight()/2f,0);			
			light.attachToBody(body, 0,0);			
		} 
		
		//Setup sensors.
		for (AgentSensor sensor: sensors) {
			if (sensor.fixture == null) {

				//from http://www.iforce2d.net/b2dtut/sensors
				//add semicircle radar sensor to tower
				PolygonShape polygonShape = new PolygonShape();
				float radius = (17f+sensor.radius) * getScaleX();
				//float angle = sensor.angle ...
				Vector2[] vertices = new Vector2[8];
				vertices[0] = new Vector2(0f+sensor.offsetVector.x, 0f+sensor.offsetVector.y);
				
				float startAngle = sensor.offset_degrees + (sensor.angle * 0.5f) ;
				
				for (int i=0; i<7; i++) {
					float angle =  (i / 6.0f * sensor.angle * DEGTORAD)-(startAngle*DEGTORAD);
					vertices[i+1] = (new Vector2((radius * (float) Math.cos(angle))+sensor.offsetVector.x, (radius * (float) Math.sin(angle))+sensor.offsetVector.y));
				}
				polygonShape.set(vertices);
				
				FixtureDef sensorFixture = new FixtureDef();
				sensorFixture.shape = polygonShape;
				sensorFixture.density = 0.0f;
				sensorFixture.friction = 0.0f;
				sensorFixture.restitution = 0.0f;
				sensorFixture.isSensor = true;
				
				sensorFixture.filter.categoryBits = FILTER_SENSOR;
				sensorFixture.filter.maskBits = FILTER_AGENT | FILTER_AGENT_TWO | FILTER_AGENT_THREE;
				
				sensor.fixture = body.createFixture(sensorFixture);
				sensor.fixture.setUserData(sensor);
			}
			
			if (sensor.shouldBeRemoved) {
				body.destroyFixture(sensor.fixture);
				sensors.removeValue(sensor, true);
			}
		}
		
		//Manage Joints
		for (AgentJoint joint: joints) {
			if (joint.joint == null) {
				
				DistanceJointDef jointDef = new DistanceJointDef();
				jointDef.type = JointDef.JointType.DistanceJoint;

				jointDef.bodyA = body;
				jointDef.bodyB = joint.connectedAgent.body;
				
				jointDef.length = joint.length;
				jointDef.dampingRatio = joint.damping;
				jointDef.frequencyHz = 30;
				
				jointDef.collideConnected = false;
				
				joint.joint = arena.world.createJoint(jointDef);
				
				
			} else if (joint.shouldBeDestroyed) {
				//destroy joint
				arena.world.destroyJoint(joint.joint);
				joint.joint = null;				
				joints.removeValue(joint, true);
				
			}
		}
		
		//update position.
		 float wrappedRotation = (float) WrapPosNegPI((double)rotation);
		 rotation = wrappedRotation;
		 
		 Vector2 current  = body.getPosition();
		 currentPosition.x  = current.x;
		 currentPosition.y  = current.y;
		 
		 float bodyAngle = body.getAngle();
		 float desiredAngle = rotation;
		 float totalRotation = desiredAngle - bodyAngle;
		 float change = (float) Math.PI*2; //allow 1 degree rotation per time step
		 float newAngle = bodyAngle + Math.min( change, Math.max(-change, totalRotation));
		 currentAngle = newAngle;
		 
		 float x=(float) (-speed*Math.sin((double)newAngle));
		 float y=(float) (speed*Math.cos((double)newAngle));
			 
		if (updateAgentPosition) {
			 body.setTransform(current, newAngle);
			 body.setLinearVelocity(new Vector2(y, -x));
			 body.getFixtureList().get(0).getShape().setRadius(getScaleX()*(getWidth()/2.4f));
		 
		}
		 //Call abstract class.
		 agentAct(delta);
	}
      
    /**
     * Create a joint with the specified agent. willCreateJointWithAgent() is called for ourselves
     * and the other agent prior to creation.
     * @param agent
     * @param length
     * @param damping
     * @return
     */
    public boolean createJointWithAgent(Agent agent, float length, float damping) {
    	if (willCreateJointWithAgent(agent)) {
    		if (agent.willCreateJointWithAgent(this)) {
    	
    	    directlyConnectedAgents.add(agent);
    	    willCreateJointWithAgent(agent);
    	    
    	    agent.directlyConnectedAgents.add(this);
    	    agent.willCreateJointWithAgent(this);
    	    
    	    willCreateJointWithAgent(null);
    	    agent.willCreateJointWithAgent(null);


    	    for (Agent agentD: allConnectedAgents) {
    	    	agentD.willCreateJointWithAgent(null);
    	    }
    	    
    	    for (Agent agentD: agent.allConnectedAgents) {
    	    	agentD.willCreateJointWithAgent(null);
    	    }

        	AgentJoint agentJoint = new AgentJoint();
        	agentJoint.connectedAgent  = agent;
        	agentJoint.length = length;
        	agentJoint.damping = damping;
        	
        	joints.add(agentJoint);
           	return true;
    	
    		}
    	}
    	
    	return false;
    }
    
    /**
     * Destroy the joint with the specified agent.
     * @param agent
     */
    public void destroyJointWithAgent(Agent agent) {
    	for (AgentJoint joint: joints) {
    		if (joint.connectedAgent == agent) {
				assert(directlyConnectedAgents.removeValue(agent, true));
    			joint.shouldBeDestroyed = true; //TODO: srsly, can this even work?
		    	assert(willCreateJointWithAgent(null));
    		}
    	}
    }
    
    /**
     * Determine if an agent would create a joint with another agent.
     * This can be overwritten to prohibit creation of joints between agents.
     * @param agent
     * @return
     */
    public boolean willCreateJointWithAgent(Agent agent) {
    	for (Agent connectedAgent: directlyConnectedAgents) {
			if (connectedAgent == agent) {
				return false;
			}
    	}
    	    	
    	agentsConnected.clear();
    	agentsConnected.addAll(directlyConnectedAgents);
    	
    	do {
    		agentsToAddToList.clear();
    		
    		for (Agent agent_: agentsConnected) {
    			Array<Agent> agentsConnectedToConnected = agent_.directlyConnectedAgents;
    			for(Agent agent__: agentsConnectedToConnected) {
    				if (!agentsConnected.contains(agent__, true) && !agentsToAddToList.contains(agent__, true)) {
    					agentsToAddToList.add(agent__);
    				}
    			}
    			
    		}
    		
    		agentsConnected.addAll(agentsToAddToList);
    		
    	} while (agentsToAddToList.size>0);
    	
    	allConnectedAgents.clear();
    	allConnectedAgents.addAll(agentsConnected);
    	
    	return true;
    }
    
    /**
     * Go forward by a certain speed.
     * @param givenSpeed
     */
    public void goForward(float givenSpeed) {
    	speed = givenSpeed;
    }
    
    /**
     * Go backward by a certain speed.
     * @param givenSpeed
     */
    public void goBackward(float givenSpeed) {
    	speed = -givenSpeed;
    }
    
    /**
     * Stop all movement.
     */
    public void stop() {
    	speed = 0.0f;
    }
    
    /**
     * Turn left by the specified angle in radians.
     * @param angle
     */
    public void turnLeft(float angle) {
    	rotation += angle;
    }
    
    /**
     * Turn right by the specified angle in radians.
     * @param angle
     */
    public void turnRight(float angle) {
    	rotation -= angle;
    }
    
    public void setAngle(float angle) {
    	rotation = angle;
    }

    
    /**
     * Update density of agent.
     * @param density
     */
    public void setDensity(float density) {
    	if (fixture != null) {
    		fixture.setDensity(density);
    		body.resetMassData();
    		//Gdx.app.log("Agent: ", "Can only update density in constructor.");
    	} else {
    		initDensity = density;
    	}
    }
    
    /**
     * Update friction of agent.
     * @param friction
     */
    public void setFriction(float friction) {
    	if (fixture != null) {
    		fixture.setFriction(friction);
    		//Gdx.app.log("Agent: ", "Can only update friction in constructor.");
    	} else {
    		initFriction = friction;
    	}   
    }
    
    /**
     * Update restitution of agent.
     * @param restitution
     */
    public void setRestitution(float restitution) {
    	if (fixture != null) {
    		fixture.setRestitution(restitution);
    		body.resetMassData();
    		Gdx.app.log("Agent: ", "Can only update restitution in constructor.");
    	} else {
    		initRestitution = restitution;
    	}   
    }
    
    /**
     * Adds a sensor to the agent.
     * @param sensor
     */
    public void addSensor(AgentSensor sensor) {
    	sensors.insert(sensor.id, sensor);
    }
    
    /**
     * Removes a sensor from the agent.
     * @param sensor
     */
    public void removeSensor(AgentSensor sensor) {
    	sensors.removeIndex(sensor.id);
    }
    
    /**
     * Gets a sensor of the agent.
     * @param sensor
     */
    public void getSensor(AgentSensor sensor) {
    	sensors.get(sensor.id);
    }
    
    /**
     * Gets a sensor of the agent from an id.
     * @param sensor
     */
    public AgentSensor getSensor(int id) {
    	return sensors.get(id);
    }
    
    
    /**
     * Remove and cleanup the agent.
     */
    public void die() {
        if (light != null) {
        	light.remove();
        }
    	
    	arena.world.destroyBody(this.body);

    	this.remove();
    }
        
    /**
     * Gets the angle to another agent.
     * @param agent
     * @return
     */
    public float getAngleToAgent(Agent agent) {
    	double want_angle = Math.atan2(agent.getY()-getY(), agent.getX()-getX());
		want_angle -= Math.PI/2.0f;
		double finalAngle = WrapPosNegPI(want_angle);
		finalAngle *= 180 / Math.PI;    	
		
    	return (float) finalAngle;
    }
    
    /**
     * Gets the distance to another agent.
     * TODO: Implement this with raycasting?
     * @param agent
     * @return
     */
    public double getDistanceToAgent(Agent agent) {
    	double xDiff = getX()-agent.getX();
    	double yDiff = getY()-agent.getY();
    	
    	return Math.sqrt((xDiff*xDiff) + (yDiff*yDiff));
    }
    
    /**
     * Gets the distance to a point.
     * TODO: Implement this with raycasting?
     * @param x
     * @param y
     * @return
     */
    public double getDistanceToPoint(float x, float y) {
    	double xDiff = getX()-x;
    	double yDiff = getY()-y;
    	
    	return Math.sqrt((xDiff*xDiff) + (yDiff*yDiff));
    }
    
    /**
     * Set the amount of Blur linearly.
     * @param x
     * @param y
     */
    public void setBlurXY(float x, float y) {
    	blurAmountX = x;
    	blurAmountY = y;
    }
    
    /**
     * Set the amount of Blur linearly.
     * @param f
     */
    public void setBlur(float f) {
    	blurAmountX = f;
    	blurAmountY = f;
    }
    
	
	public double Mod(double x, double y)
	{
	    if (0 == y)
	        return x;
	    return x - y * Math.floor(x/y);
	}

	// wrap [rad] angle to [-PI..PI)
	public double WrapPosNegPI(double fAng)
	{
	    return Mod(fAng + Math.PI, Math.PI*2) - Math.PI;
	}
	
	/**
	 * 
	 * UNTESTED
	 * 
	 */
	public float getLightIntensity() {
		return arena.rayHandler.intensityAtPoint(getX(), getY());
		
	}

 }
